#include <windows.h>
#include "crtmath.h"
#include <time.h>
#include "../dxut/dxstdafx.h"
#include <algorithm>

#include "crt.h"
#include "crtutil.h"

const COLOR COLOR::BLACK(0, 0, 0);
const COLOR COLOR::WHITE(255, 255, 255);


void COLOR::Randomize()
{
	static bool needInit = true;

	if (needInit)
	{
		needInit = false;
		srand( (unsigned int)time( NULL ) );
	}

	r = rand() & 0x000000ff;
	g = rand() & 0x000000ff;
	b = rand() & 0x000000ff;
	a = rand() & 0x000000ff;
}

bool RayIntersectTriangle( const RAY& ray, TCRTTriangleHelper& TriangleHelper, float &u, float &v, float &dist )
{
	bool hit = false;

	VEC3 IP;
	float Distance;
	if (TriangleHelper.plane.Intersect(ray, IP, Distance))
	{
		float t1 = IP.Dot(TriangleHelper.perp2)/* * TriangleHelper.a1*/ + TriangleHelper.b1;

		if (t1 >= 0 && t1 <= 1.0f)
		{
			float t2 = IP.Dot(TriangleHelper.perp1)/* * TriangleHelper.a2*/ + TriangleHelper.b2;

			if (t2 >= 0 && t2 <= 1.0f && (t1 + t2) <= 1.0f)
			{	
				hit = true;
				u = t1;
				v = t2;
				dist = Distance;
			}
		} 
	}
	return hit;
}



// Distance, distance from the ray origin to the nearest hit point
//bool RayIntersectAABB(RAY& ray, AABB& aabb, float& Distance, TOctree* pTree)
//{
//	bool hit = false;
//
//	VEC3 edgeSize = aabb.GetSize() * 2.0f;
//	float Radius2 = (edgeSize * 0.5f).Size();
//
//	// Quick rejection: when distance between the AABB center and the ray is less than radius
//	float Point2Ray = 0;
//	VEC3  TempRay = aabb.GetOrigin() - ray.origin;	// from ray origin to sphere center
//	float Projection = TempRay.Dot(ray.direction);
//
//	if (Projection >= 0)
//	{
//		Point2Ray = (TempRay - ray.direction * Projection).Size();
//	}
//	else
//	{
//		Point2Ray = TempRay.Size();
//	}
//
//	if (Point2Ray >= Radius2)
//	{
//		hit = false;
//	}
//	else if (aabb.ContainPoint(ray.origin)) 
//	{
//		Distance = 0;
//		hit = true;
//	}
//	else
//	{
//		FRectangle rectangles[6];
//
//		BuildRectanges(aabb, rectangles);
//
//		for (int i = 0; i < 6; i++)
//		{
//			FRectangle& r = rectangles[i];
//
//			if (!r.Plane.AtFront(ray.origin))
//			{
//				VEC3 IP;	// intersection point
//				float dist;
//
//				if (r.Plane.Intersect(ray, IP, dist))
//				{
//					VEC3 temp = IP - r.Origin;
//					float proj0 = temp.Dot(r.V0);
//					float proj1 = temp.Dot(r.V1);
//
//					if (proj0 >= 0 && proj0 <= r.D0 
//						&& proj1 >= 0 && proj1 <= r.D1)
//					{
//						hit = true;
//						Distance = dist;
//						break;
//					}
//				}
//			}
//		}
//	}
//
//	return hit;
//}


#define LEAF_INDEX(depth, x, y, z) \
	((z << (2 * depth - 2)) + (y << (depth - 1)) + x)

#define BIG_NUMBER	(99999999.0f)

// The tree aabb is uniformed split into size*size*size leaves. (size = 1 << (treeDepth-1))
// Each leaf has an associated index, the index is z - y - x majoyed.
// Check which leaves will be hit by the ray (ordered by hit sequence)
//bool RayIntersectTree(RAY& ray, AABB& rootAABB, int treeDepth, vector<int>& hitLeafArray, TOctree* pTree)
//{
//	int size = 1 << (treeDepth - 1);
//	hitLeafArray.clear();
//
//	VEC3 leafSize = rootAABB.GetSize() * (2.0f / float(size));
//	int x, y, z;
//	float t0;
//	bool hit = RayIntersectAABB(ray, rootAABB, t0, pTree);
//
//	if (hit)
//	{
//		int leafIndex;
//		VEC3 hitPoint = ray.origin + ray.direction * t0 - rootAABB.minV;	// in AABB local space
//
//		x = min(int(hitPoint.x / leafSize.x), size-1);
//		y = min(int(hitPoint.y / leafSize.y), size-1);
//		z = min(int(hitPoint.z / leafSize.z), size-1);
//
//		float invX = IsZero(ray.direction.x) ? BIG_NUMBER : 1.0f / ray.direction.x;
//		float invY = IsZero(ray.direction.y) ? BIG_NUMBER : 1.0f / ray.direction.y;
//		float invZ = IsZero(ray.direction.z) ? BIG_NUMBER : 1.0f / ray.direction.z;
//
//		int deltaX = (ray.direction.x >= 0) ? 1 : -1;
//		int deltaY = (ray.direction.y >= 0) ? 1 : -1;
//		int deltaZ = (ray.direction.z >= 0) ? 1 : -1;
//
//		int curEdgeX = ray.direction.x > 0 ? x : (x + 1);
//		int curEdgeY = ray.direction.y > 0 ? y : (y + 1);
//		int curEdgeZ = ray.direction.z > 0 ? z : (z + 1);
//
//		float nextEdgeX, nextEdgeY, nextEdgeZ;
//		float tTogoX, tTogoY, tTogoZ;
//		float minTogo;
//		//leafIndex = LEAF_INDEX(treeDepth, x, y, z);
//		//hitLeafArray.push_back( leafIndex );
//
//		//float nextEdgeX = (curEdgeX + deltaX) * leafSize.x;
//		//float nextEdgeY = (curEdgeY + deltaY) * leafSize.y;
//		//float nextEdgeZ = (curEdgeZ + deltaZ) * leafSize.z;
//
//		//float tTogoX = (nextEdgeX - hitPoint.x) * invX;
//		//float tTogoY = (nextEdgeY - hitPoint.y) * invY;
//		//float tTogoZ = (nextEdgeZ - hitPoint.z) * invZ;
//
//		//float minTogo = min(min(tTogoX, tTogoY), tTogoZ);
//		//hitPoint += ray.direction * minTogo;
//		//if (fEqual(tTogoX, minTogo))
//		//{
//		//	x += deltaX;
//		//	curEdgeX += deltaX;
//		//}
//		//if (fEqual(tTogoY, minTogo))
//		//{
//		//	y += deltaY;
//		//	curEdgeY += deltaY;
//		//}
//		//if (fEqual(tTogoZ, minTogo))
//		//{
//		//	z += deltaZ;
//		//	curEdgeZ += deltaZ;
//		//}
//
//		while (x >= 0 && x < size && y >= 0 && y < size && z >= 0 && z < size)
//		{
//			leafIndex = LEAF_INDEX(treeDepth, x, y, z);
//			hitLeafArray.push_back( leafIndex );
//
//			nextEdgeX = (curEdgeX + deltaX) * leafSize.x;	
//			nextEdgeY = (curEdgeY + deltaY) * leafSize.y;
//			nextEdgeZ = (curEdgeZ + deltaZ) * leafSize.z;
//
//			tTogoX = (nextEdgeX - hitPoint.x) * invX;
//			tTogoY = (nextEdgeY - hitPoint.y) * invY;
//			tTogoZ = (nextEdgeZ - hitPoint.z) * invZ;
//
//			minTogo = min(min(tTogoX, tTogoY), tTogoZ);
//			hitPoint += ray.direction * minTogo;
//			if (fEqual(tTogoX, minTogo))
//			{
//				x += deltaX;
//				curEdgeX += deltaX;
//			}
//			if (fEqual(tTogoY, minTogo))
//			{
//				y += deltaY;
//				curEdgeY += deltaY;
//			}
//			if (fEqual(tTogoZ, minTogo))
//			{
//				z += deltaZ;
//				curEdgeZ += deltaZ;
//			}
//		}
//	}
//
//	return hit;
//}


Frustum::Frustum(VEC3& eyePos, VEC3& viewDir, VEC3& viewUp, float nearClip, float farClip, float fovX, float fovY) :
	eye(eyePos),
	view(viewDir),
	up(viewUp),
	nearclip(nearClip),
	farclip(farClip),
	fovx(fovX),
	fovy(fovY)
{
	aspect = tanf(0.5f * fovx) / tanf(0.5f * fovy);

	UpdateVertex();
}

	Frustum::Frustum(VEC3& eyePos, VEC3& viewDir, VEC3 &viewUp, vector<AABB>& aabb)
	{
		eye = eyePos;
		view = viewDir;
		up = viewUp;

		// Build up the view matrix
		D3DXMATRIXA16	View;
		D3DXVECTOR3		EyePos( &eyePos );
		D3DXVECTOR3		ViewDir( &viewDir );
		D3DXVECTOR3		ViewUp( &viewUp );
		D3DXVECTOR3		EyeTarget = EyePos + ViewDir;

		D3DXMatrixLookAtLH( &View, &EyePos, &EyeTarget, &ViewUp );

		fovx = 0;
		fovy = 0;
		nearclip	= FLT_MAX;
		farclip		= -FLT_MAX;
		for (size_t i = 0; i < aabb.size(); i++)
		{
			AABB& TheAABB = aabb[i];

			for (int j = 0; j < 8; j++)
			{
				D3DXVECTOR3	Vertex(j&1?TheAABB.minV.x:TheAABB.maxV.x, 
					j&2?TheAABB.minV.y:TheAABB.maxV.y,
					j&4?TheAABB.minV.z:TheAABB.maxV.z);

				D3DXVECTOR3 VertexInView;
				D3DXVec3TransformCoord( &VertexInView, &Vertex, &View );

				nearclip = min(nearclip, VertexInView.z);
				farclip = max(farclip, VertexInView.z);

				fovx = max(fovx, fabs(VertexInView.x / VertexInView.z));
				fovy = max(fovy, fabs(VertexInView.y / VertexInView.z));
			}
		}

		aspect = fovx / fovy;

		fovx = atanf(fovx) * 2.0f;
		fovy = atanf(fovy) * 2.0f;

		UpdateVertex();
	}

	Frustum::Frustum(VEC3& eyePos, VEC3& viewDir, VEC3 &viewUp, Frustum& f)
	{
		eye = eyePos;
		view = viewDir;
		up = viewUp;

		// Build up the view matrix
		D3DXMATRIXA16	View;
		D3DXVECTOR3		EyePos( &eyePos );
		D3DXVECTOR3		ViewDir( &viewDir );
		D3DXVECTOR3		ViewUp( &viewUp );

		D3DXVECTOR3		EyeTarget = EyePos + ViewDir;
		D3DXMatrixLookAtLH( &View, &EyePos, &EyeTarget, &ViewUp );

		fovx = 0;
		fovy = 0;
		nearclip	= FLT_MAX;
		farclip		= -FLT_MAX;
		for (int i = 0; i < 8; i++)
		{
			D3DXVECTOR3 Vertex(&f.vert[i]);
			D3DXVECTOR3 VertexInView;
			D3DXVec3TransformCoord( &VertexInView, &Vertex, &View );

			nearclip = min(nearclip, VertexInView.z);
			farclip = max(farclip, VertexInView.z);

			fovx = max(fovx, fabs(VertexInView.x / VertexInView.z));
			fovy = max(fovy, fabs(VertexInView.y / VertexInView.z));
		}

		aspect = fovx / fovy;

		fovx = atanf(fovx) * 2.0f;
		fovy = atanf(fovy) * 2.0f;

		UpdateVertex();
	}

	Frustum::Frustum(VEC3& eyePos, VEC3& viewDir, VEC3 &viewUp, vector<VEC3>& vertices)
	{
		eye = eyePos;
		view = viewDir;
		up = viewUp;

		// Build up the view matrix
		D3DXMATRIXA16	View;
		D3DXVECTOR3		EyePos( &eyePos );
		D3DXVECTOR3		ViewDir( &viewDir );
		D3DXVECTOR3		ViewUp( &viewUp );
		D3DXVECTOR3		EyeTarget = EyePos + ViewDir;

		D3DXMatrixLookAtLH( &View, &EyePos, &EyeTarget, &ViewUp );

		fovx = 0;
		fovy = 0;
		nearclip	= FLT_MAX;
		farclip		= -FLT_MAX;
		for (size_t i = 0; i < vertices.size(); i++)
		{
			VEC3& Vertex = vertices[i];

			D3DXVECTOR3 VertexInView;
			D3DXVECTOR3 _T((float*)&Vertex);
			D3DXVec3TransformCoord( &VertexInView, &_T, &View );

			nearclip = min(nearclip, VertexInView.z);
			farclip = max(farclip, VertexInView.z);

			fovx = max(fovx, fabs(VertexInView.x / VertexInView.z));
			fovy = max(fovy, fabs(VertexInView.y / VertexInView.z));
		}

		aspect = fovx / fovy;

		fovx = atanf(fovx) * 2.0f;
		fovy = atanf(fovy) * 2.0f;

		UpdateVertex();
	}

Frustum::Frustum(VEC3& eyePos, VEC3& viewDir, VEC3 &viewUp, VEC3 vertices[], int nVertex)
{
	eye = eyePos;
	view = viewDir;
	up = viewUp;

	// Build up the view matrix
	D3DXMATRIXA16	View;
	D3DXVECTOR3		EyePos( &eyePos );
	D3DXVECTOR3		ViewDir( &viewDir );
	D3DXVECTOR3		ViewUp( &viewUp );

	D3DXVECTOR3	EyeTarget = (EyePos + ViewDir);
	D3DXMatrixLookAtLH( &View, &EyePos, &EyeTarget, &ViewUp );

	fovx = 0;
	fovy = 0;
	nearclip	= FLT_MAX;
	farclip		= -FLT_MAX;
	for (int i = 0; i < nVertex; i++)
	{
		VEC3& Vertex = vertices[i];

		D3DXVECTOR3 VertexInView;
		D3DXVECTOR3 _T((float*)&Vertex);
		D3DXVec3TransformCoord( &VertexInView, &_T, &View );

		nearclip = min(nearclip, VertexInView.z);
		farclip = max(farclip, VertexInView.z);

		fovx = max(fovx, fabs(VertexInView.x / VertexInView.z));
		fovy = max(fovy, fabs(VertexInView.y / VertexInView.z));
	}

	aspect = fovx / fovy;

	fovx = atanf(fovx) * 2.0f;
	fovy = atanf(fovy) * 2.0f;

	UpdateVertex();
}

void Frustum::UpdateVertex()
{
	VEC3 left	= up.CrossL(view);	
	VEC3 center;

	float tanx = tanf(fovx * 0.5f);
	float tany = tanf(fovy * 0.5f);

	VEC3 upV, leftV;

	// near plane
	center	= eye + view * nearclip;
	upV		= up * tany * nearclip;
	leftV	= left * tanx * nearclip;
	vert[0] = center + upV + leftV;
	vert[1] = center + upV - leftV;
	vert[2] = center - upV + leftV;
	vert[3] = center - upV - leftV;

	// far plane
	center	= eye + view * farclip;
	upV		= up * tany * farclip;
	leftV	= left * tanx * farclip;
	vert[4] = center + upV + leftV;
	vert[5] = center + upV - leftV;
	vert[6] = center - upV + leftV;
	vert[7] = center - upV - leftV;
}

int Frustum::triangle_list[12 * 3] = 
{
	0, 2, 1, 1, 2, 3, 1, 3, 5, 5, 3, 7,
	0, 1, 5, 0, 5, 4, 0, 4, 2, 4, 6, 2, 
	7, 3, 2, 7, 2, 6, 5, 7, 4, 4, 7, 6,
};

int Frustum::line_list[12 * 2] = 
{
	0, 1, 1, 3, 3, 2, 2, 0, 
	4, 5, 5, 7, 7, 6, 6, 4, 
	0, 4, 1, 5, 3, 7, 2, 6,
};

// Transform one bounding box into another space
void TransformBoundingBox(D3DXMATRIXA16& View, D3DXVECTOR3& MinV, D3DXVECTOR3& MaxV, D3DXVECTOR3& NewMinV, D3DXVECTOR3& NewMaxV)
{
	NewMinV = D3DXVECTOR3(FLT_MAX, FLT_MAX, FLT_MAX);
	NewMaxV = -NewMinV;

	for (int i = 0; i < 8; i++)
	{
		D3DXVECTOR3 V = D3DXVECTOR3( (i&1) ? MinV.x : MaxV.x, (i&2) ? MinV.y : MaxV.y, (i&4) ? MinV.z : MaxV.z);

		D3DXVECTOR3 TransformedV;
		D3DXVec3TransformCoord( &TransformedV, &V, &View );

		D3DXVec3Minimize( &NewMinV, &NewMinV, &TransformedV );
		D3DXVec3Maximize( &NewMaxV, &NewMaxV, &TransformedV );
	}
}



bool FPlane::Intersect(VEC3& P0, VEC3& P1, VEC3& Output)
{
	// Line = P0 + t(P1 - P0)
	VEC3 Delta = P1 - P0;

	// Line - Plane intersection
	float t = (-Distance -P0.Dot(Normal)) / Delta.Dot(Normal);

	bool Result = false;
	if (t >= CRT_MATH_EPSILON && t <= 1.0f - CRT_MATH_EPSILON)
	{
		Output = P0 + Delta * t;
		Result = true;
	}

	return Result;
}

bool FPlane::Intersect(const struct RAY& Ray, VEC3& IP, float& IPDistance)
{
	bool Result = false;
	float d = Ray.direction.Dot(Normal);

	if (d <= CRT_MATH_EPSILON && d >= -CRT_MATH_EPSILON)
	{
		Result = false;
	}
	else
	{
		float dist = (-Distance - Ray.origin.Dot(Normal)) / d;
		
		Result = (dist >= 0);

		if (Result)
		{
			IP = Ray.origin + Ray.direction * dist;
			IPDistance = dist;
		}
	}

	return Result;
}

// construct CP from 4 co-plane vertices
FConvexPlane::FConvexPlane(VEC3 P0, VEC3 P1, VEC3 P2, VEC3 P3)
{
	Vertices[0] = P0;
	Vertices[1] = P1;
	Vertices[2] = P2;
	Vertices[3] = P3;
	nVertex = 4;

	VEC3 V0 = P1 - P0;
	V0.Normalize();

	VEC3 V1 = P2 - P1;
	V1.Normalize();

	Plane.Normal	= V0.CrossL(V1);
	Plane.Distance	= -Plane.Normal.Dot(P0);
}
//
//FConvexPlane::FConvexPlane(VEC3& P0, VEC3& P1, VEC3& P2, VEC3& P3)
//{
//	Vertices[0] = P0;
//	Vertices[1] = P1;
//	Vertices[2] = P2;
//	Vertices[3] = P3;
//	nVertex = 4;
//
//	VEC3 V0 = P1 - P0;
//	V0.Normalize();
//
//	VEC3 V1 = P2 - P1;
//	V1.Normalize();
//
//	Plane.Normal	= V0.CrossL(V1);
//	Plane.Distance	= -Plane.Normal.Dot(P0);
//}


void FConvexPlane::AddVertex(VEC3& P)
{
	for (int i = 0; i < nVertex; i++)
	{
		// It's already in the array
		if ((Vertices[i] - P).Size() < CRT_MATH_EPSILON)
			return;
	}

	Vertices[nVertex++] = P;
}


struct FSortAtomic
{
	float	Angle;
	VEC3	Vertex;
};

int AngleLessThan(const void* A, const void* B)
{
	return ((FSortAtomic*)A)->Angle < ((FSortAtomic*)B)->Angle;
}

void FConvexPlane::SortVertices()
{
	if (nVertex <= 2)
		return;

	FSortAtomic	Buffer[CONVEX_MAX_VERTEX];

	VEC3 Center = (Vertices[0] + Vertices[1] + Vertices[2]) * (1.0f / 3.0f);	
	// We use vertices[0] as the anchor
	VEC3 V0 = Vertices[0] - Center;
	V0.Normalize();

	for (int i = 0; i < nVertex; i++)
	{
		Buffer[i].Vertex = Vertices[i];

		// Angle between two vectors, range [0, 2PI]
		// http://blog.csdn.net/happy__888/archive/2005/04/28/367008.aspx
		
		VEC3 V1 = (Vertices[i] - Center);
		V1.Normalize();
		Buffer[i].Angle  = atan2f( V0.CrossR(V1).Dot(Plane.Normal), V0.Dot(V1) );
	}

	qsort(Buffer, nVertex, sizeof(FSortAtomic), AngleLessThan);

	for (int i = 0; i < nVertex; i++)
	{
		Vertices[i] = Buffer[i].Vertex;
	}
}

void FConvexPlane::Flip()
{
	Plane.Normal = -Plane.Normal;

	for (int i = 0; i < nVertex / 2; i++)
		Vertices[i] = Vertices[nVertex - 1 - i];
}

void FConvexPlane::SplitByPlane(FConvexPlane& FrontCP, FConvexPlane& BackCP, VEC3& P0, VEC3& P1, FPlane& SplitPlane)
{
	if (nVertex < 3)
		return;

	FrontCP.Init(Plane);
	BackCP.Init(Plane);

	int FirstEdge = -1;			// P0 on this edge
	int SecondEdge = -1;		// P1 on this edge

	float Distances[CONVEX_MAX_VERTEX + 1];
	for (int i = 0; i < nVertex; i++)
	{
		Distances[i] = SplitPlane.Normal.Dot(Vertices[i]) + SplitPlane.Distance;
	}
	Distances[nVertex] = Distances[0];


	bool FirstVertexAtFront = (Distances[0] >= 0.0f);
	FConvexPlane& FirstCP	= FirstVertexAtFront ? FrontCP : BackCP;
	FConvexPlane& SecondCP	= FirstVertexAtFront ? BackCP : FrontCP;

	for (int i = 0; i < nVertex; i++)
	{
		VEC3& Ref0 = Vertices[i];
		FirstCP.Vertices[FirstCP.nVertex++] = Ref0;

		if (Distances[i] * Distances[i+1] < 0)
		{
			VEC3& Ref1 = (i == nVertex-1) ? Vertices[0] : Vertices[i + 1];
			float Ratio = fabs(Distances[i+1]) / (fabs(Distances[i]) + fabs(Distances[i+1]));

			P0 = Ref0 * Ratio + Ref1 * (1.0f - Ratio);
			FirstCP.Vertices[FirstCP.nVertex++] = P0;
			SecondCP.Vertices[SecondCP.nVertex++] = P0;

			FirstEdge = i;
			break;
		}
	}

	if (FirstEdge != -1)
	{
		// Search the second intersection
		for (int i = FirstEdge + 1; i < nVertex; i++)
		{
			VEC3& Ref0 = Vertices[i];
			SecondCP.Vertices[SecondCP.nVertex++] = Ref0;

			if (Distances[i] * Distances[i+1] < 0)
			{
				VEC3& Ref1 = (i == nVertex-1) ? Vertices[0] : Vertices[i + 1];
				float Ratio = fabs(Distances[i+1]) / (fabs(Distances[i]) + fabs(Distances[i+1]));

				P1 = Ref0 * Ratio + Ref1 * (1.0f - Ratio);
				FirstCP.Vertices[FirstCP.nVertex++] = P1;
				SecondCP.Vertices[SecondCP.nVertex++] = P1;

				SecondEdge = i;
				break;
			}
		}

		if (SecondEdge == -1)
		{
			// Only intersect with a vertex (extreme case)
			VEC3 Center = (Vertices[0] + Vertices[1]) * 0.5f;

			if (SplitPlane.AtFront(Center))
			{
				BackCP.nVertex = 0;
				FrontCP = *this;
				//FrontCP.nVertex = nVertex;
				//for (int i = 0; i < nVertex; i++)
				//	FrontCP.Vertices[i] = Vertices[i];
			}
			else
			{
				FrontCP.nVertex = 0;
				BackCP = *this;
				//BackCP.nVertex = nVertex;
				//for (int i = 0; i < nVertex; i++)
				//	BackCP.Vertices[i] = Vertices[i];
			}
		}
		else
		{
			for (int i = SecondEdge + 1; i < nVertex; i++)
			{
				FirstCP.Vertices[FirstCP.nVertex++] = Vertices[i];
			}
		}
	}
}

int FConvexHull::OutputVertex(VEC3 Output[], bool AllowRedundant)
{
	int N = 0;

	if (AllowRedundant)
	{
		for (int i = 0; i < nPlane; i++)
		{
			FConvexPlane& CP = Planes[i];
			for (int j = 0; j < CP.nVertex; j++)
			{
				Output[N++] = CP.Vertices[j];
			}
		}
	}
	else
	{
		for (int i = 0; i < nPlane; i++)
		{
			FConvexPlane& CP = Planes[i];
			for (int j = 0; j < CP.nVertex; j++)
			{
				VEC3& P0 = CP.Vertices[j];
				bool Found = false;

				for (int k = 0; k < N; k++)
				{
					VEC3& P1 = Output[k];

					if ((P0 - P1).Size() < CRT_MATH_EPSILON)
					{
						Found = true;
						break;
					}
				}

				if (!Found)
					Output[N++] = P0;
			}
		}
	}

	return N;
}

void FConvexHull::SplitByPlane(FConvexHull& FrontHull, FConvexHull& BackHull, FPlane& SplitPlane)
{
	FrontHull.Clear();
	BackHull.Clear();

	FConvexPlane SplitCP;
	SplitCP.Init(SplitPlane);

	for (int i = 0; i < nPlane; i++)
	{
		FConvexPlane& CP = Planes[i];
		VEC3 P0, P1;
		FConvexPlane FrontCP, BackCP;

		CP.SplitByPlane(FrontCP, BackCP, P0, P1, SplitPlane);

		if (!FrontCP.IsEmpty())
			FrontHull.AppendConvexPlane(FrontCP);

		if (!BackCP.IsEmpty())
			BackHull.AppendConvexPlane(BackCP);

		if (!FrontCP.IsEmpty() && !BackCP.IsEmpty())
		{
			SplitCP.AddVertex(P0);
			SplitCP.AddVertex(P1);
		}
	}

	if (!SplitCP.IsEmpty())
	{
		SplitCP.SortVertices();
		FrontHull.AppendConvexPlane(SplitCP);

		SplitCP.Flip();
		BackHull.AppendConvexPlane(SplitCP);
	}
}

void FConvexHull::IntersectByConvexHull(FConvexHull& Result, FConvexHull& InConvex)
{
	Result = *this;

	if (!Result.IsEmpty())
	{
		for (int i = 0; i < InConvex.nPlane; i++)
		{
			FConvexPlane& CP = InConvex.Planes[i];

			FConvexHull InHull, OutHull;
			Result.SplitByPlane(InHull, OutHull, CP.Plane);
			Result = InHull;

			if (Result.IsEmpty())
				break;
		}
	}
}

FConvexHull::FConvexHull(Frustum& F, bool NeedUpperPlane)
{
	nPlane = 0;

	// near
	Planes[nPlane++] = FConvexPlane(F.vert[0], F.vert[1], F.vert[3], F.vert[2]);	
	// far
	Planes[nPlane++] = FConvexPlane(F.vert[4], F.vert[6], F.vert[7], F.vert[5]);
	// left
	Planes[nPlane++] = FConvexPlane(F.vert[0], F.vert[2], F.vert[6], F.vert[4]);
	// right
	Planes[nPlane++] = FConvexPlane(F.vert[1], F.vert[5], F.vert[7], F.vert[3]);
	// upper
	if (NeedUpperPlane)
		Planes[nPlane++] = FConvexPlane(F.vert[5], F.vert[1], F.vert[0], F.vert[4]);
	// lower
	Planes[nPlane++] = FConvexPlane(F.vert[2], F.vert[3], F.vert[7], F.vert[6]);
}

FConvexHull::FConvexHull(AABB& B)
{
	nPlane = 0;

	// near
	Planes[nPlane++] = FConvexPlane(B.Vertex(5), B.Vertex(4), B.Vertex(6), B.Vertex(7));
	// far
	Planes[nPlane++] = FConvexPlane(B.Vertex(0), B.Vertex(1), B.Vertex(3), B.Vertex(2));
	// left
	Planes[nPlane++] = FConvexPlane(B.Vertex(1), B.Vertex(5), B.Vertex(7), B.Vertex(3));
	// right
	Planes[nPlane++] = FConvexPlane(B.Vertex(4), B.Vertex(0), B.Vertex(2), B.Vertex(6));
	// upper
	Planes[nPlane++] = FConvexPlane(B.Vertex(4), B.Vertex(5), B.Vertex(1), B.Vertex(0));
	// lower
	Planes[nPlane++] = FConvexPlane(B.Vertex(7), B.Vertex(6), B.Vertex(2), B.Vertex(3));
}
